﻿using System;
using OpenCVForUnity.CoreModule;
using OpenCVForUnity.ImgprocModule;
using UnityEngine;
using UnityEngine.UI;
using Point = OpenCVForUnity.CoreModule.Point;

public class BodyMeasureController: MonoBehaviour{
    
    private PoseEstimationController m_PoseEstimationController;
    public PoseEstimationController poseEstimationController
    {
        get
        {
            if (m_PoseEstimationController == null)
                m_PoseEstimationController = gameObject.GetComponentInChildren<PoseEstimationController>();
            if (m_PoseEstimationController == null)
                m_PoseEstimationController = gameObject.AddComponent<PoseEstimationController>();
            return m_PoseEstimationController;
        }
    }  
   
    public RawImage output;
    public RawImage output2;

    private void Awake()
    {
        var pe = poseEstimationController;
    }

    private void Start()
    {
        
    }

    public void CalMeasureData(Texture2D frontTex,Texture2D sideTex,float tall,float weight,out FrontData frontData,out  SideData sideData
        ,out Mat outputTex1 ,out Mat outputTex2)
    {
        frontData = null;
        sideData = null;
        outputTex1 = null;
        outputTex2 = null;
        
        Mat frontPoseMat= null;
        Mat frontOutlineMat= null;
        Mat sidePoseMat= null;
        Mat sideOutlineMat= null;
        OutlineData frontOutlineData = null;
        KeyPoints frontPosePoints= null;
        OutlineData sideOutlineData= null;
        KeyPoints sidePoseData= null;
        
        var mat = poseEstimationController.PoseEstimation(frontTex,out frontPoseMat,out frontOutlineMat,true);

        if (frontPoseMat != null && frontOutlineMat != null)
        {
            poseEstimationController.GetOutlinePose(frontPoseMat,frontOutlineMat
            , out frontOutlineData,out frontPosePoints);
            
            if(mat != null)
                poseEstimationController. poseEstimator.visualize_mask(mat,frontOutlineMat);
        }
        var mat2 = poseEstimationController.PoseEstimation(sideTex,out sidePoseMat,out sideOutlineMat,true);
        if (sidePoseMat != null && sideOutlineMat != null)
        {
            poseEstimationController.GetOutlinePose(sidePoseMat,sideOutlineMat
            ,out sideOutlineData,out sidePoseData);
            
            if(mat2 != null)
                poseEstimationController. poseEstimator.visualize_mask(mat2,sideOutlineMat);
            else 
                GameDebug.LogError($"mat2 is null");
        }
        else
        {
            GameDebug.LogError($"sidePoseMat:{sidePoseMat != null} sideOutlineMat:{sideOutlineMat != null}");
        }

        // 使用cvtColor函数将BGR转为RGB
        Imgproc.cvtColor(mat, mat, Imgproc.COLOR_BGR2RGB);
        Imgproc.cvtColor(mat2, mat2, Imgproc.COLOR_BGR2RGB);
        outputTex1 = mat;
        outputTex2 = mat2;
        
        if (frontOutlineData != null && frontPosePoints != null && sideOutlineData != null && sidePoseData != null)
        {
            try
            {
                frontData = poseEstimationController.GetFrontData(frontOutlineData, frontPosePoints);
                sideData = poseEstimationController.GetSideData(frontData, sideOutlineData, sidePoseData);
                
                //poseEstimationController. poseEstimator.visualize_mask(mat,frontOutlineMat);
                //poseEstimationController. poseEstimator.visualize_mask(mat2,sideOutlineMat);
            }
            catch (Exception e)
            {
                GameDebug.LogException(e);
                frontData = null;
                sideData = null;
            }
        }
        GameDebug.LogError($"front data:{frontData}  sideData:{sideData}");
    }

    public void AddResult2DebugView(Mat mat,FrontData frontData,float realTall)
    {
        Scalar line_color = new Scalar(0, 0, 255, 255);
        
        float rate = realTall/ frontData.tall;
        float shoulderWidth = frontData.backShoulderWidth * rate;
        float calChestWidth = frontData.chestWidth * rate;
        float calWaistWidth = frontData.waistWidth * rate;
        float calHipWidth = frontData.hipWidth * rate;
        
        int thickness = 2;
        int height = mat.rows();
        Imgproc.line(mat, new Point(frontData.backShouldLeftPoint.x,height - frontData.backShouldLeftPoint.y), new Point(frontData.backShouldRightPoint.x,height-frontData.backShouldRightPoint.y), line_color, thickness);
        var shoulderCenterPos = (frontData.backShouldLeftPoint + frontData.backShouldLeftPoint) / 2;
        Imgproc.putText(mat, $"{shoulderWidth:F1}cm", new Point(shoulderCenterPos.x,height - shoulderCenterPos.y - 5),  
            Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 0, 0, 255), 2, Imgproc.LINE_AA, false);
        
        Imgproc.line(mat, new Point(frontData.chestLeftPoint.x,height - frontData.chestLeftPoint.y), new Point(frontData.chestRightPoint.x,height-frontData.chestRightPoint.y), line_color, thickness);
        var chestCenterPos = (frontData.chestLeftPoint + frontData.chestRightPoint) / 2;
        Imgproc.putText(mat, $"{calChestWidth:F1}cm", new Point(chestCenterPos.x,height - chestCenterPos.y - 5),  
            Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 0, 0, 255), 2, Imgproc.LINE_AA, false);
        
        Imgproc.line(mat, new Point(frontData.waistLeftPoint.x,height - frontData.waistLeftPoint.y), new Point(frontData.waistRightPoint.x,height-frontData.waistRightPoint.y), line_color, thickness);
        var waistCenterPos = (frontData.waistLeftPoint + frontData.waistRightPoint) / 2;
        Imgproc.putText(mat, $"{calWaistWidth:F1}cm", new Point(waistCenterPos.x,height - waistCenterPos.y - 5),  
            Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 0, 0, 255), 2, Imgproc.LINE_AA, false);
        
        Imgproc.line(mat, new Point(frontData.hipLeftPoint.x,height - frontData.hipLeftPoint.y), new Point(frontData.hipRightPoint.x,height-frontData.hipRightPoint.y), line_color, thickness);
        var hipCenterPos = (frontData.hipLeftPoint + frontData.hipRightPoint) / 2;
        Imgproc.putText(mat, $"{calHipWidth:F1}cm", new Point(hipCenterPos.x,height - hipCenterPos.y - 5),  
            Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 0, 0, 255), 2, Imgproc.LINE_AA, false);
        
        Imgproc.line(mat, new Point(hipCenterPos.x,height - frontData.footPointY), new Point(hipCenterPos.x,height - frontData.topPointY), new Scalar(255, 0, 255, 255), thickness);
    }
    public void AddResult2DebugView(Mat mat,SideData sideData,float realTall)
    {
        Scalar line_color = new Scalar(0, 0, 255, 255);
        int thickness = 2;
        
        float rate = realTall/ sideData.tall;
        float calChestWidth = sideData.chestDepth * rate;
        float calWaistWidth = sideData.waisDepth * rate;
        float calHipWidth = sideData.hipDepth * rate;
        
        int height = mat.rows();
        Imgproc.line(mat, new Point(sideData.chestLeftPoint.x,height - sideData.chestLeftPoint.y), new Point(sideData.chestRightPoint.x,height-sideData.chestRightPoint.y), line_color, thickness);
        var chestCenterPos = (sideData.chestLeftPoint + sideData.chestRightPoint) / 2;
        Imgproc.putText(mat, $"{calChestWidth:F1}cm", new Point(chestCenterPos.x,height - chestCenterPos.y - 5),  
            Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 0, 0, 255), 2, Imgproc.LINE_AA, false);
           
        Imgproc.line(mat, new Point(sideData.waistLeftPoint.x,height - sideData.waistLeftPoint.y), new Point(sideData.waistRightPoint.x,height-sideData.waistRightPoint.y), line_color, thickness);
        var waistCenterPos = (sideData.waistLeftPoint + sideData.waistRightPoint) / 2;
        Imgproc.putText(mat, $"{calWaistWidth:F1}cm", new Point(waistCenterPos.x,height - waistCenterPos.y - 5),  
            Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 0, 0, 255), 2, Imgproc.LINE_AA, false);
        
        Imgproc.line(mat, new Point(sideData.hipLeftPoint.x,height - sideData.hipLeftPoint.y), new Point(sideData.hipRightPoint.x,height-sideData.hipRightPoint.y), line_color, thickness);
        var hipCenterPos = (sideData.hipLeftPoint + sideData.hipRightPoint) / 2;
        Imgproc.putText(mat, $"{calHipWidth:F1}cm", new Point(hipCenterPos.x,height - hipCenterPos.y - 5),  
            Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 0, 0, 255), 2, Imgproc.LINE_AA, false);
        
        Imgproc.line(mat, new Point(hipCenterPos.x,height - sideData.footPointY), new Point(hipCenterPos.x,height - sideData.topPointY), new Scalar(255, 0, 255, 255), thickness);
    }

    public float testWidth = 28;
    public float testDepth = 18.2f;
    public float testHeight = 170;
    public float testWeight = 58f;
    [ContextMenu("TestCalChestCir")]
    public void TestCalChestCir()
    {
        float bodyMass = BodyMassIndex(testHeight, testWeight);
        float result = CalChestCir(testWidth, testDepth, bodyMass);
        
        Debug.Log($"TestCalChestCir  result:{result}");
    }
    [ContextMenu("TestCalWaistCir")]
    public void TestCalWaistCir()
    {
        float bodyMass = BodyMassIndex(testHeight, testWeight);
        float result = CalWaistCir(testWidth, testDepth, bodyMass);
        Debug.Log($"TestCalWaistCir  result:{result}");
    }
    [ContextMenu("TestCalHipCir")]
    public void TestCalHipCir()
    {
        float bodyMass = BodyMassIndex(testHeight, testWeight);
        float result = CalHipCir(testWidth, testDepth, bodyMass);
        Debug.Log($"TestCalHipCir  result:{result}");
    }
    public static float CalChestCir(float frontWidth, float sideDepth,float bodyMassFactor)
    {
        return CalCir(frontWidth,  sideDepth, bodyMassFactor,1.05f,0.05f);
    }  
    public static float CalWaistCir(float frontWidth, float sideDepth,float bodyMassFactor)
    {
        return CalCir(frontWidth,  sideDepth, bodyMassFactor,1.07f,0.05f);
    }  
    public static  float CalHipCir(float frontWidth, float sideDepth,float bodyMassFactor)
    {
        return CalCir(frontWidth,  sideDepth, bodyMassFactor,1.04f,0.05f);
    }

    static float CalCir(float frontWidth, float sideDepth,float bodyMassFactor,float ajustK = 1.11f,float adjustMassFactor = .5f)
    {
        float k = ajustK + (bodyMassFactor - 1) * adjustMassFactor;
        float ellipticalPerimeter = EllipticalPerimeter(frontWidth, sideDepth);
        float value = ellipticalPerimeter * k;
        return value;
    }

    public static float BodyMassIndex(float realTall,float weight)
    {
        float v = weight / (realTall * realTall / 10000);
        GameDebug.Log($"tall:{realTall} weigh:{weight} index:{v}");
        return v / 21f;
    }
    /// <summary>
    /// 计算椭圆周长
    /// </summary>
    /// <param name="frontWidth"></param>
    /// <param name="sideDepth"></param>
    /// <returns></returns>
    public static float EllipticalPerimeter(float frontWidth, float sideDepth)
    {
        float a = frontWidth / 2;
        float b = sideDepth / 2;
        float h = ((a-b)*(a-b)) / ((a+b)*(a+b));
        float C = 1 + (1 / 4f) * h + (1 / 64f) * (h * h) + (1 / 256f) * (h * h * h);
        float value = Mathf.PI * (a + b) * C;
        return value;
    }
    
    
    public BodyReportData GetBodyReport(float realTall,float realWeight,FrontData frontData,SideData sideData)
    {
        float frontRate = realTall/ frontData.tall;
        float sideRate = realTall/ sideData.tall;
        
        float bodyMassFactor = BodyMassIndex(realTall, realWeight);

        float frontChestWidth = frontData.chestWidth * frontRate;
        float sideChestDepth = sideData.chestDepth * sideRate;
        float chestCir = CalChestCir(frontChestWidth, sideChestDepth,bodyMassFactor);

        float frontWaistWidth = frontData.waistWidth * frontRate;
        float sideWaistDepth = sideData.waisDepth * sideRate;
        float WaistCir = CalWaistCir(frontWaistWidth, sideWaistDepth,bodyMassFactor);
        
        float frontHipWidth = frontData.hipWidth * frontRate;
        float sideHipDepth = sideData.hipDepth * sideRate;
        float HipCir = CalHipCir(frontHipWidth, sideHipDepth,bodyMassFactor);
        
        float calShoulderWidth = frontData.backShoulderWidth * frontRate;
        
        BodyReportData data = new BodyReportData();
        data.backShoulderWidth = calShoulderWidth;
        data.chestCir = chestCir;
        data.abdominalCir = WaistCir;
        data.hipCir = HipCir;
        
        GameDebug.Log($"calShoulderWidth:{calShoulderWidth}  frontData.backShoulderWidth:{ frontData.backShoulderWidth}  realTall:{realTall}  frontData.tall:{frontData.tall}");
        
        return data;
    }
    

    private Mat testFrontOutline;
    private Mat testFrontPose;
    private Mat testSideOutline;
    private Mat testSidePose;
    public void TestPoseEstimation(Texture2D testImg1,Texture2D testImg2)
    {
        bool showMark = false;
        var mat = poseEstimationController.PoseEstimation(testImg1, out testFrontPose, out testFrontOutline, showMark);
        Texture2D outputtex = PoseEstimationController.Mat2Tex(mat);
        this.output.texture = outputtex;
        this.output.GetComponent<RectTransform>().sizeDelta = new Vector2(testFrontOutline.cols(),testFrontOutline.rows());
        
        var mat2 = poseEstimationController.PoseEstimation(testImg2, out testSidePose, out testSideOutline, showMark);
        Texture2D outputtex2 = PoseEstimationController.Mat2Tex(mat2);
        this.output2.texture = outputtex2;
        this.output2.GetComponent<RectTransform>().sizeDelta = new Vector2(outputtex2.width,outputtex2.height);
        
        #if UNITY_EDITOR
        if (showMark)
        {
            byte[] bytes = outputtex.EncodeToPNG();
            UnityEngine.Windows.File.WriteAllBytes(Application.dataPath + "./TestImg.png", bytes);
        }
        #endif
    }

    private void OnDestroy()
    {
        this.testFrontOutline = null;
        this.testFrontPose = null;
    }

    // Update is called once per frame
    void Update()
    {
        
    }

    void DrawRect(Vector3 pos)
    {
        float size = 0.5f;
        Gizmos.DrawLine(pos + new Vector3(-size,-size),pos + new Vector3(-size,size));
        Gizmos.DrawLine(pos + new Vector3(-size,size),pos + new Vector3(size,size));
        Gizmos.DrawLine(pos + new Vector3(size,size),pos + new Vector3(size,-size));
        Gizmos.DrawLine(pos + new Vector3(size,-size),pos + new Vector3(-size,-size));
    }

    Vector2 TransformShowPoint(Transform trans,Vector2 pos)
    {
        /*
        if (this.testFrontOutline != null)
        {
            pos.y = testFrontOutline.rows() - pos.y;
        }
        */
        var size = trans.GetComponent<RectTransform>().sizeDelta;
        pos = trans.TransformPoint(pos-size/2);
        return pos;
    }

    void DrawLine(Transform trans,Vector2 dataPos1, Vector2 dataPos2)
    {
        dataPos1 = TransformShowPoint(trans,dataPos1);
        dataPos2 = TransformShowPoint(trans,dataPos2);
        Gizmos.DrawLine(dataPos1,dataPos2);
    }
    
    public bool drawGizmos = true;
    private int logged;
    private void OnDrawGizmos()
    {
        if (!drawGizmos) 
            return;
        
        FrontData frontData = null;
        if (this.testFrontOutline != null && testFrontPose != null)
        {
            poseEstimationController.GetOutlinePose(testFrontPose,testFrontOutline
                ,out OutlineData outlineData,out KeyPoints posePoints);

            frontData = poseEstimationController.GetFrontData(outlineData, posePoints);
            Gizmos.color = Color.green;
            DrawLine(output.transform,frontData.backShouldLeftPoint,frontData.backShouldRightPoint);
            DrawLine(output.transform,frontData.chestLeftPoint,frontData.chestRightPoint);
            DrawLine(output.transform,frontData.waistLeftPoint,frontData.waistRightPoint);
            DrawLine(output.transform,frontData.hipLeftPoint,frontData.hipRightPoint);
            
            Gizmos.color = Color.magenta;
            Vector2 footPoint = (posePoints.leftFootPoint + posePoints.rightFootPoint) / 2;
            DrawLine(output.transform,outlineData.topPoint,footPoint);
            
            /*float tall = outlineData.topPoint.y - footPoint.y;
            Gizmos.color = Color.magenta;
            
            var outlinePoints = outlineData.allPoints;
            
            int outlinePointsCount = 0;
            Gizmos.color = Color.yellow;

            for (int row = 0; row < outlinePoints.GetLength(1); row++)
            {
                for (int col = 0; col < outlinePoints.GetLength(0); col++)
                {
                    if (outlinePoints[col, row] > 0)
                    {
                        outlinePointsCount++;
                    }
                }
            }

            //检测肩宽
            float shoulderCheckRate = 0.03f;
            int shoulderCheckCount = (int)(shoulderCheckRate * tall);
            float shoulderWidth = 0;
            Vector2  maxShouldWidthLeftPoint = new Vector2(float.PositiveInfinity,float.PositiveInfinity);
            Vector2  maxShouldWidthRightPoint = Vector2.zero;
            int checkShoulderBeginY = (int)posePoints.leftShoulderPoint.y;
            for (int row = checkShoulderBeginY; row <= checkShoulderBeginY+ shoulderCheckCount; row++)
            {
                var curShoulderLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
                var curShoulderRightPoint = Vector2.zero;
                float chestWidthOnElbowY=0;

                for (int col = 0; col < outlinePoints.GetLength(0); col++)
                {
                    if (outlinePoints[col,row] > 0)
                    {
                        var posInt = new Vector2Int(col, row);
                        var pos = new Vector2(posInt.x, posInt.y);

                        if (pos.x < curShoulderLeftPoint.x)
                        {
                            curShoulderLeftPoint = pos;
                        }
                        if (pos.x > curShoulderRightPoint.x)
                        {
                            curShoulderRightPoint = pos;
                        }
                        pos = TransformShowPoint(pos);
                        DrawRect(pos);
                    }
                }

                float curShoulerWidth = curShoulderRightPoint.x - curShoulderLeftPoint.x;
                if (shoulderWidth < curShoulerWidth)
                {
                    shoulderWidth = curShoulerWidth;
                    maxShouldWidthLeftPoint = curShoulderLeftPoint;
                    maxShouldWidthRightPoint = curShoulderRightPoint;
                } 
            }
            
            //检测胸宽
            float chestCheckRate = 0.08f;
            int chestCheckCount = (int)(chestCheckRate * tall);
            float chestWidth = 0;
            Vector2 chestLeftPoint = Vector2.zero;
            Vector2 chestRightPoint = Vector2.zero;
            Vector2 chestCenterPoint = (posePoints.leftElbowPoint + posePoints.rightElbowPoint) / 2;
            Vector2Int chestCenterPointInt = new Vector2Int((int)chestCenterPoint.x, (int)chestCenterPoint.y);
            int chestCheckBeginRow = (int)posePoints.leftElbowPoint.y;
            for (int row = chestCheckBeginRow; row <= chestCheckBeginRow+chestCheckCount; row++)
            {
                var curChestLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
                var curChestRightPoint = Vector2.zero;

                for (int col = 0; col < outlinePoints.GetLength(0); col++)
                {
                    if (outlinePoints[col,row] > 0)
                    {
                        var posInt = new Vector2Int(col, row);
                        var pos = new Vector2(posInt.x, posInt.y);

                        bool connectToArm = false;
                        if (posInt.x > chestCenterPointInt.x)
                        {
                            Vector2Int leftElbowInt = new Vector2Int((int)posePoints.leftElbowPoint.x,(int)posePoints.leftElbowPoint.y);
                            connectToArm = ConnectToPoint(outlinePoints,posInt, leftElbowInt);
                        }
                        else
                        {    
                            Vector2Int rightElbowInt = new Vector2Int((int)posePoints.rightElbowPoint.x,(int)posePoints.rightElbowPoint.y);
                            connectToArm = ConnectToPoint(outlinePoints,posInt, rightElbowInt);
                        }
                        if (ConnectToPoint(outlinePoints, posInt, chestCenterPointInt) && !connectToArm)
                        {
                            if (pos.x < curChestLeftPoint.x)
                            {
                                curChestLeftPoint = pos;
                            }

                            if (pos.x > curChestRightPoint.x)
                            {
                                curChestRightPoint = pos;
                            } 
                            Gizmos.color = Color.yellow;
                        }
                        else
                        {
                            Gizmos.color = Color.red;
                        }
                        
                        pos = TransformShowPoint(pos);
                        DrawRect(pos);
                    }
                }
                float curChestWidth = curChestRightPoint.x - curChestLeftPoint.x;
                if (chestWidth < curChestWidth)
                {
                    if (chestWidth <= 0 || (curChestWidth - chestWidth) < (0.04f * tall))
                    {
                        chestWidth = curChestWidth;
                        chestLeftPoint = curChestLeftPoint;
                        chestRightPoint = curChestRightPoint;
                    }
                }
            }
            
            //检测臀宽
            float hipCheckRate = 0.03f;
            int hipCheckCount = (int)(hipCheckRate * tall);
            float hipWidth = 0;
            Vector2  hipLeftPoint = new Vector2(float.PositiveInfinity,float.PositiveInfinity);
            Vector2  hipRightPoint = Vector2.zero;
            int checkHipBeginY = (int)posePoints.leftThighPoint.y;
            Vector2 hipCenterPoint = (posePoints.leftThighPoint + posePoints.rightThighPoint) / 2;
            Vector2Int hipCenterPointInt = new Vector2Int((int)hipCenterPoint.x,(int)hipCenterPoint.y);
            for (int row = checkHipBeginY - hipCheckCount; row <= checkHipBeginY+ hipCheckCount; row++)
            {
                var curHipLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
                var curHipRightPoint = Vector2.zero;

                for (int col = 0; col < outlinePoints.GetLength(0); col++)
                {
                    if (outlinePoints[col,row] > 0)
                    {
                        var posInt = new Vector2Int(col, row);
                        var pos = new Vector2(posInt.x, posInt.y);

                        if (ConnectToPoint(outlinePoints, posInt, hipCenterPointInt))
                        {
                            if (pos.x < curHipLeftPoint.x)
                            {
                                curHipLeftPoint = pos;
                            }
                            if (pos.x > curHipRightPoint.x)
                            {
                                curHipRightPoint = pos;
                            }
                        }

                        pos = TransformShowPoint(pos);
                        DrawRect(pos);
                    }
                }

                float curHipWidth = curHipRightPoint.x - curHipLeftPoint.x;
                if (hipWidth < curHipWidth)
                {
                    hipWidth = curHipWidth;
                    hipLeftPoint = curHipLeftPoint;
                    hipRightPoint = curHipRightPoint;
                } 
            }
            
            //检测腰宽
            float WaistCheckRate = 0.03f;
            int WaistCheckCount = (int)(WaistCheckRate * tall);
            float waistWidth = 0;
            Vector2  WaistLeftPoint = new Vector2(float.PositiveInfinity,float.PositiveInfinity);
            Vector2  WaistRightPoint = Vector2.zero;
            int checkWaistBeginY = (checkHipBeginY + checkShoulderBeginY) / 2;
            for (int row = checkWaistBeginY - WaistCheckCount; row <= checkWaistBeginY+ WaistCheckCount; row++)
            {
                var curWaistLeftPoint = new Vector2(float.PositiveInfinity, float.PositiveInfinity);
                var curWaistRightPoint = Vector2.zero;
                float chestWidthOnElbowY=0;

                for (int col = 0; col < outlinePoints.GetLength(0); col++)
                {
                    if (outlinePoints[col,row] > 0)
                    {
                        var posInt = new Vector2Int(col, row);
                        var pos = new Vector2(posInt.x, posInt.y);

                        bool connectToArm = false;
                        if (posInt.x > chestCenterPointInt.x)
                        {
                            Vector2Int leftElbowInt = new Vector2Int((int)posePoints.leftElbowPoint.x,(int)posePoints.leftElbowPoint.y);
                            connectToArm = ConnectToPoint(outlinePoints,posInt, leftElbowInt);
                        }
                        else
                        {    
                            Vector2Int rightElbowInt = new Vector2Int((int)posePoints.rightElbowPoint.x,(int)posePoints.rightElbowPoint.y);
                            connectToArm = ConnectToPoint(outlinePoints,posInt, rightElbowInt);
                        }

                        if (ConnectToPoint(outlinePoints, posInt, chestCenterPointInt) && !connectToArm)
                        {
                            if (pos.x < curWaistLeftPoint.x)
                            {
                                curWaistLeftPoint = pos;
                            }

                            if (pos.x > curWaistRightPoint.x)
                            {
                                curWaistRightPoint = pos;
                            }
                        }

                        pos = TransformShowPoint(pos);
                        Gizmos.color = Color.magenta;
                        DrawRect(pos);
                    }
                }

                float curWaistWidth = curWaistRightPoint.x - curWaistLeftPoint.x;
                if (waistWidth <= 0 || waistWidth > curWaistWidth)
                {
                    waistWidth = curWaistWidth;
                    WaistLeftPoint = curWaistLeftPoint;
                    WaistRightPoint = curWaistRightPoint;
                } 
            }
            
            Gizmos.color = Color.green;
            DrawLine(maxShouldWidthLeftPoint,maxShouldWidthRightPoint);
            DrawLine(chestLeftPoint,chestRightPoint);
            DrawLine(hipLeftPoint,hipRightPoint);
            DrawLine(WaistLeftPoint,WaistRightPoint);

            Gizmos.color = Color.black;
            //Gizmos.DrawSphere(TransformShowPoint(chestCenterPoint),1f);
            Gizmos.DrawSphere(TransformShowPoint(hipCenterPoint),1f);
            
            if (logged < 1)
            {
                logged++;
                GameDebug.Log($"outline points count:{outlinePointsCount}  shoulderCheckCount:{shoulderCheckCount}  tall:{tall}  top point:{outlineData.topPoint} footPoint:{footPoint} ");
            }*/
            
            Gizmos.color = Color.white;
            //draw pose key points
            var poseAllPoints = posePoints.allPoints;
            for (int j = 0; j <poseAllPoints.Count; ++j)
            {
                var pos = poseAllPoints[j];
                pos = TransformShowPoint(output.transform,pos);
                DrawRect(pos);
            }
            
            DrawLine(output.transform,posePoints.leftShoulderPoint,posePoints.rightShoulderPoint);
            DrawLine(output.transform,posePoints.leftShoulderPoint,posePoints.leftElbowPoint);
            DrawLine(output.transform,posePoints.leftElbowPoint,posePoints.leftWristPoint);
            DrawLine(output.transform,posePoints.rightShoulderPoint,posePoints.rightElbowPoint);
            DrawLine(output.transform,posePoints.rightElbowPoint,posePoints.rightWristPoint);
            DrawLine(output.transform,posePoints.leftShoulderPoint,posePoints.leftThighPoint);
            DrawLine(output.transform,posePoints.rightShoulderPoint,posePoints.rightThighPoint);
            DrawLine(output.transform,posePoints.leftThighPoint,posePoints.leftCalfPoin);
            DrawLine(output.transform,posePoints.leftCalfPoin,posePoints.leftAnklePoint);
            DrawLine(output.transform,posePoints.leftAnklePoint,posePoints.leftFootPoint);
            DrawLine(output.transform,posePoints.leftFootPoint,posePoints.leftToePoint);
            DrawLine(output.transform,posePoints.leftToePoint,posePoints.leftAnklePoint);
            DrawLine(output.transform,posePoints.rightThighPoint,posePoints.rightCalfPoint);
            DrawLine(output.transform,posePoints.rightCalfPoint,posePoints.rightAnklePoint);
            DrawLine(output.transform,posePoints.rightAnklePoint,posePoints.rightFootPoint);
            DrawLine(output.transform,posePoints.rightFootPoint,posePoints.rightToePoint);
            DrawLine(output.transform,posePoints.rightToePoint,posePoints.rightAnklePoint);
        }
        if (this.testSideOutline != null && testSidePose != null)
        {
            poseEstimationController.GetOutlinePose(testSidePose,testSideOutline
                ,out OutlineData outlineData,out KeyPoints posePoints);

            if (outlineData == null || posePoints == null)
                return;

            SideData sideData = poseEstimationController.GetSideData(frontData, outlineData ,posePoints);
            Gizmos.color = Color.green;
            DrawLine(output2.transform,sideData.chestLeftPoint,sideData.chestRightPoint);
            DrawLine(output2.transform,sideData.waistLeftPoint,sideData.waistRightPoint);
            DrawLine(output2.transform,sideData.hipLeftPoint,sideData.hipRightPoint);
            
            Gizmos.color = Color.magenta;
            Vector2 footPoint = (posePoints.leftFootPoint + posePoints.rightFootPoint) / 2;
            DrawLine(output2.transform,outlineData.topPoint,footPoint);
            
            var outlinePoints = outlineData.allPoints;
            
            int outlinePointsCount = 0;
            Gizmos.color = Color.red;

            for (int row = 0; row < outlinePoints.GetLength(1); row++)
            {
                for (int col = 0; col < outlinePoints.GetLength(0); col++)
                {
                    if (outlinePoints[col, row] > 0)
                    {
                        outlinePointsCount++;
                        
                        /*var posInt = new Vector2Int(col, row);
                        var pos = new Vector2(posInt.x, posInt.y);
                        pos = TransformShowPoint(output2.transform,pos);
                        DrawRect(pos);*/
                    }
                }
            }
            if (logged < 1)
            {
                logged++;
                GameDebug.Log($"outline points count:{outlinePointsCount} tall:{sideData.tall} front Tall:{frontData.tall} top point:{outlineData.topPoint} footPoint:{footPoint} ");
            }
            Gizmos.color = Color.white;
            //draw pose key points
            var poseAllPoints = posePoints.allPoints;
            for (int j = 0; j <poseAllPoints.Count; ++j)
            {
                var pos = poseAllPoints[j];
                pos = TransformShowPoint(output2.transform,pos);
                DrawRect(pos);
            }
            
            DrawLine(output2.transform,posePoints.leftShoulderPoint,posePoints.rightShoulderPoint);
            DrawLine(output2.transform,posePoints.leftShoulderPoint,posePoints.leftElbowPoint);
            DrawLine(output2.transform,posePoints.leftElbowPoint,posePoints.leftWristPoint);
            DrawLine(output2.transform,posePoints.rightShoulderPoint,posePoints.rightElbowPoint);
            DrawLine(output2.transform,posePoints.rightElbowPoint,posePoints.rightWristPoint);
            DrawLine(output2.transform,posePoints.leftShoulderPoint,posePoints.leftThighPoint);
            DrawLine(output2.transform,posePoints.rightShoulderPoint,posePoints.rightThighPoint);
            DrawLine(output2.transform,posePoints.leftThighPoint,posePoints.leftCalfPoin);
            DrawLine(output2.transform,posePoints.leftCalfPoin,posePoints.leftAnklePoint);
            DrawLine(output2.transform,posePoints.leftAnklePoint,posePoints.leftFootPoint);
            DrawLine(output2.transform,posePoints.rightThighPoint,posePoints.rightCalfPoint);
            DrawLine(output2.transform,posePoints.rightCalfPoint,posePoints.rightAnklePoint);
            DrawLine(output2.transform,posePoints.rightAnklePoint,posePoints.rightFootPoint);
        }
    }

   
}